Fall prevention algorithm